COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	TITLE TSERVO ROUTINE
C00004 00003	CONSTANTS
C00006 00004		LAC 10,[XWD 5,P1]
C00007 00005	HANG:	TROA 17,HUNG+DONE
C00008 ENDMK
C⊗;
TITLE TSERVO ROUTINE
	STATUS↑:	0	;STATUS BITS
	FLAG↑:		0	;NON-ZERO IF SERVOING
	COUNT↑:		0	;LENS CHANGE COUNTER
	P1↑:0↔	P2↑:0↔	P3↑:0	;LATEST POT READING - FOCUS,TILT,PAN
	X1↑:0↔	X2↑:0↔	X3↑:0	;PREVIOUS POT READING
	L1↑:0↔	L2↑:0↔	L3↑:0	;FINAL POT VALUES
	E1↑:4↔	E2↑:10↔	E3↑:10	;TOLERANCES

;REGISTERS FOR INTERNAL USE ONLY
	DEL:	25		;COUNTER FOR HUNG A-D
	MCNT:	300		;LENS COUNT
	MCNTX:	240		;LENS MOTOR CUTOFF
	MISSD:	100		;DATA MISSED COUNT
	CNTR:	40		;STOP COUNTER MAX

;STATUS BITS
	DONE←←1
	RUN←←2
	HUNG←←20
	STOP←←10
	LENS←←4
	MISS←←40

;RELAY BITS
	P2M←←1B18	;TILT UP
	P2P←←1B19	;TILT DOWN
	P3P←←1B20	;PAN LEFT
	P3M←←1B21	;PAN RIGHT
	P1M←←1B22	;FOCUS FAR
	P1P←←1B23	;FOCUS NEAR
	XLENS←←1B24	;ACCUATE TURRET
;CONSTANTS
	AD←←424
	DATA←←204
	MISC←←700
	REL←←40
OPDEF SPCWAR[43B8]
OPDEF LAC[MOVE]
OPDEF DAC[MOVEM]
OPDEF GO[JRST]

;UPDATE STATUS BITS
TSERVO:	LAC 17,STATUS↔ANDCMI 17,HUNG+MISS
	IORI 17,RUN
	SETZM FLAG
	SETZM 16;
	TRNN 17,DONE↔GO L1
	SETZM COUNT
	CONO MISC,REL
	DAC 17,STATUS
	DISMIS

L1:	TRZN 17,LENS↔GO L2
	LAC MCNT
	ADDM COUNT
L2:	TRNE 17,STOP↔GO LD
	CONI DATA,1
	ANDI 1,7
	CONO DATA,4250
	CONO AD,172000
	LAC 4,MISSD
	LAC 3,DEL
	CONI DATA,2
	TRNE 2,11000↔GO +3
	SOJG 3,-3
	GO HANG
	TRNN DATA,10000↔GO +3
	SOJG 4,-10
	GO DMISS
	DATAI DATA,5
	CONO AD,4111
	CONO DATA,203560(1)

	ASHC 5,-=24
	LSH 6,1
	ASHC 6,-=24
	LSH 7,1
	ASH 7,-=24
	LAC 10,[XWD 5,P1]
	BLT 10,P3
	SUB 5,L1
	SUB 6,L2
	SUB 7,L3
	MOVM 11,5
	MOVM 12,6
	MOVM 13,7
	CAMGE 11,E1↔GO L3

	JUMPG 5,+2
	TROA 16,P1M
	ORI 16,P1P
	SETOM FLAG
L3:	CAMGE 12,E2↔GO L4
	JUMPG 6,+2
	TROA 16,P2M
	IORI 16,P2P
	SETOM FLAG
L4:	CAMGE 13,E3↔GO L5
	JUMPG 7,+2
	TROA 16,P3M
	ORI 16,P3P
	GO L6

L5:	SKIPN FLAG↔GO LD
L6:	MOVS 10,[XWD 5,P1]↔BLT 10,7	;FETCH
	SUB 5,X1↔SUB 6,X2↔SUB 7,X3	;DIFFERENCE
	MOVMS 5↔MOVMS 6↔MOVMS 7		;ABSOLUTE VALUE.
	SUB 5,E1↔SUB 6,E2↔SUB 7,E3	;TOLERANCE.
	AND 5,6↔AND 5,7↔JUMPGE 5,L10
	AOS 15,SCNT
	CAMGE 15,CNTR↔GO L11
	SETZM 16
	IORI 17,STOP+DONE
	GO .+5
HANG:	TROA 17,HUNG+DONE
DMISS:	IORI 17,MISS+DONE
	CONO AD,4000
	CONO DATA,203560(1)
	SETZM COUNT
	SETZM SCNT#
	GO LB

LD:	SKIPN COUNT↔IORI 17,DONE
L10:	LAC 10,[XWD P1,X1]↔BLT 10,X3
	SETZM SCNT
L11:	LAC 15,COUNT
	CAMLE 15,MCNTX↔IORI 16,XLENS
LB:	CONO MISC,REL(16)
	JUMPLE 15,.+2
	SOS COUNT
	DAC 17,STATUS
	DISMIS

FLUSH:	CONO MISC,REL
	SETOM FL#
	DISMIS
END